#ifndef __DALELIB_AGENT_H__
#define __DALELIB_AGENT_H__

//#include "main.h"
#include <dalelib/graveyard/agentbone.h>
#include <dalelib/maths.h>
//#include "mesh.h"
//#include "poser.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>

using namespace std;
using namespace dalelib;

struct Pose
{
	vector<float> _data;
};

struct Animation
{
	float _frame_duration;
	typedef vector<Pose*> PoseList;
	PoseList _poses;
	PoseList& getPoses() { return _poses; }
	~Animation()
	{
		for(unsigned int i=0; i<_poses.size(); ++i) delete _poses[i];
		_poses.clear();
	}
	float& getFrameDuration() { return _frame_duration; }
};

class Agent
{
public:

	typedef vector<AgentBone*> BoneList;
	typedef map<string,AgentBone*> BoneMap;
	typedef vector<Animation*> AnimationList;

private:

	BoneList _bone_list;
	BoneMap _bone_map;


	AnimationList _animations;

	float _pose_time;
	int _current_animation;
	bool _float_length;

	v3 _translate;
	v3 _rotate;
	v3 _scale;

	bool _playing;

	vector<m4> _transforms; // just a linear array of bone transforms for passing to shader

	int _total_channels;

	bool _perform_skinning;

	float _angle_units;

public:

	Agent()
	{
		reset();
	}

	~Agent()
	{
		reset();
	}

	void init()
	{
		_perform_skinning = true;
		_current_animation = 0;
		_pose_time = 0.f;
		update();
		_pose_time = 0.f;
		setGlobalMatRestInv();
	}

	BoneList& getBoneList() { return _bone_list; }
	BoneMap& getBoneMap() { return _bone_map; }
	v3& getTranslate() { return _translate; }
	v3& getRotate() { return _rotate; }
	v3& getScale() { return _scale; }
	bool& getPerformSkinning() { return _perform_skinning; }

	void setDoubleLength(const bool _DLength)
	{
		const float length_modifier = (_float_length) ? 2.f : 1.f;
		for(unsigned int i=0; i<_bone_list.size(); ++i)
		{
			_bone_list[i]->getLengthModifier() = length_modifier;

		}

		_float_length = _DLength;
	};

	m4* getTransformArray() { return &_transforms[0]; }

	int& getCurrentAnimation() { return _current_animation; }

	AnimationList& getAnimations() { return _animations; }

	bool& getPlaying() { return _playing; }

	void setGlobalMatRestInv()
	{
		for(unsigned int i=0; i<_bone_list.size(); ++i)
		{
			_bone_list[i]->setGlobalMatRestInv();
		}
	}

	void reset()
	{
		_playing = false;
        _translate = v3(0,0,0);
        _rotate = v3(0,0,0);
        _scale = v3(1,1,1);

		_float_length = false;
		_pose_time = 0.f;
		for( BoneList::iterator i=_bone_list.begin(); i!=_bone_list.end(); ++i ) if(*i) delete *i; _bone_list.clear();
		for( int i=0; i<_animations.size(); ++i ) delete _animations[i]; _animations.clear();
	}

	v3 getCentre()
	{
//		return _bones[0]->getPos()*_scale;
		return _bone_list[0]->getGlobalPos();
	}

	AgentBone* addBone(AgentBone* _Parent, const v3 _Start=v3(0,0,0), const v3 _End=v3(0,1,0), const v3 _Col=v3(1,1,1), const float _Scl=1.f)
	{
		AgentBone* b = new AgentBone( _Parent, _Start, _End, _Col, _Scl );
		_bone_list.push_back( b );
		return b;
	}

	AgentBone* getBone(const int _Index) { return _bone_list[_Index]; }
	AgentBone* getBone(const string _Name) { return _bone_map[_Name]; }

	int getNumBones() { return _bone_list.size(); }
//	bool isPlaying() { return (_poses.size()>0) && (_pose_time<float(_poses.size()-1)); }
	bool isPlaying() { return true; } // temporary

	m4 buildRotationMatrix(const v3 _Rot)
	{
		m4 mat = m4::identity;
		mat = mat*m4::rotationZ(_Rot[2]);
		mat = mat*m4::rotationY(_Rot[1]);
		mat = mat*m4::rotationX(_Rot[0]);
	}

	void loadSkeletonBVH(const string _Name)
	{
		reset();

		ifstream fin( _Name.c_str() );
		if( !fin ) return;

		v3 pos, tgt, col;
		float scl;
		int parent;
		string str;
		AgentBone* curr_bone = 0;
		int brackets = 0;
		_total_channels = 0;
		string prev_bone_name("Root");

		do
		{
			// *** add bone ***
			if(!str.compare("ROOT") || !str.compare("JOINT") || !str.compare("End"))
			{
				curr_bone = addBone( curr_bone );
				string tempstr; fin >> tempstr;
				if(!str.compare("End"))
					str = prev_bone_name+str;
				else
					str = tempstr;
				prev_bone_name = str;
				curr_bone->setName(str);
				curr_bone->getID() = _bone_list.size()-1;
				_bone_map[str] = curr_bone;
			}

			if(!str.compare("{"))
			{
				brackets++;
			}

			if(!str.compare("}"))
			{
				brackets--;
				if(brackets>0) curr_bone = curr_bone->getParent();
			}

			if(!str.compare("OFFSET"))
			{
				fin >> str; float x = atof(str.c_str());
				fin >> str; float y = atof(str.c_str());
				fin >> str; float z = atof(str.c_str());
				curr_bone->getOffset() = v3(x,y,z);
			}

			if(!str.compare("CHANNELS"))
			{
				fin >> str; int channels = atoi(str.c_str());
				_total_channels += channels;
				for(int i=0; i<channels; ++i)
				{
					fin >> str;
					if(!str.compare("Xposition")) { curr_bone->setChannel(i,0); }
					if(!str.compare("Yposition")) { curr_bone->setChannel(i,1); }
					if(!str.compare("Zposition")) { curr_bone->setChannel(i,2); }
					if(!str.compare("Xrotation")) { curr_bone->setChannel(i,3); }
					if(!str.compare("Yrotation")) { curr_bone->setChannel(i,4); }
					if(!str.compare("Zrotation")) { curr_bone->setChannel(i,5); }
				}
			}

			fin >> str;

		} while( !fin.eof() && str.compare("RESTPOSE") );


		// load rest pose
		if( !str.compare("RESTPOSE") )
		{
			Animation* a = new Animation();
			_animations.push_back(a);

			a->getFrameDuration() = 1.f;

			Pose* p = new Pose();
			a->getPoses().push_back(p);
			for(int i=0; i<_total_channels; ++i)
			{
				fin >> str; float f = atof(str.c_str());
				p->_data.push_back(f);
			}
		}


		fin.close();

		_transforms.resize(_bone_list.size());

		cout << "Agent: number of bones = " << _bone_list.size() << endl;

	}

	void loadAnimationBVH(const string _Name)
	{
		ifstream fin( _Name.c_str() );
		if( !fin ) return;

		string str;

		Animation* a = new Animation();
		_animations.push_back(a);

//		do { fin >> str; } while( str.compare("MOTION") );

		int num_frames;
		fin >> str; fin >> str; num_frames = atoi(str.c_str()); // a->getNumFrames() = atoi(str.c_str());
		fin >> str; fin >> str; fin >> str; a->getFrameDuration() = atof(str.c_str());

		for(int j=0; j<num_frames; ++j)
		{
			Pose* p = new Pose();
			a->getPoses().push_back(p);
			for(int i=0; i<_total_channels; ++i)
			{
				fin >> str; float f = atof(str.c_str());
				p->_data.push_back(f);
			}
		}

		fin.close();
	}

	void saveAnimationBVH(const int _Index,const string _Name)
	{
		ofstream fout( _Name.c_str() );
		if( !fout ) return;

		if(_Index>=_animations.size()) return;

		Animation* a = _animations[_Index];

		fout << "Frames: " << a->getPoses().size() << endl;
		fout << "Frame Time: " << a->getFrameDuration() << endl;

		for(int j=0; j<a->getPoses().size(); ++j)
		{
			Pose* p = a->getPoses()[j];

			for(int i=0; i<_total_channels; ++i)
			{
				fout << p->_data[i] << " ";
			}
			fout << endl;
		}

		fout.close();
	}


// ******************************************************************************************************
	void loadSkeletonHTR(const string _Name)
	{
		ifstream fin( _Name.c_str() );
		if( !fin ) return;


		// *** load header ***
		v3 pos, rot;
		float len;
		string str, s;
		char cstr[256];



		int num_bones, num_frames;

		{
			while( str.compare("[SegmentNames&Hierarchy]") )
			{
				fin.getline(cstr,256);
				str = string(cstr);
				istringstream iss(str);
				iss >> s;

				if(!s.compare("NumSegments")) { iss >> num_bones; }
				if(!s.compare("NumFrames")) { iss >> num_frames; }
				if(!s.compare("RotationUnits")) { iss >> s; if(!s.compare("Degrees")) _angle_units = PI/180; else _angle_units = 1.f; }
			}
			_bone_list.resize(num_bones);

			cout << "NumSegments " << num_bones << endl;
			cout << "NumFrames " << num_frames << endl;
		}

		{
			fin.getline(cstr,256);
			int i = 0;
			while( i<num_bones )
			{
				_bone_list[i] = new AgentBone(0);



				fin.getline(cstr,256);
				str = string(cstr);
				istringstream iss(str);
				iss >> s;

				if( s.length() && s[0]!='#' ) // not a comment
				{
					_bone_list[i]->_name = s; iss >> _bone_list[i]->_parent_name;
					printf("linking %s to %s\n", _bone_list[i]->_name.c_str(), _bone_list[i]->_parent_name.c_str());
					++i;
				}
			}
			for(unsigned int i=0; i<num_bones; ++i)
			{
				for(unsigned int j=0; j<num_bones; ++j)
				{
					if( !_bone_list[i]->_parent_name.compare(_bone_list[j]->_name) )
					{
						_bone_list[i]->_parent_index = j;
					}
				}


				_bone_list[i]->_parent = _bone_list[_bone_list[i]->_parent_index];
				_bone_map[_bone_list[i]->_name] = _bone_list[i];
				_bone_list[i]->getID() = i;;
			}
		}



		// *** load skeleton ***

		int i = 0;
		while(i<num_bones)
		{
			fin.getline(cstr,256);
			str = string(cstr);
			istringstream iss(str);
			iss >> s;

			if( s.length() && s[0]!='#' && s.compare("[BasePosition]") ) // not a comment
			{
//				_bones[i]._name = s;

				iss >> pos[0] >> pos[1] >> pos[2];
				iss >> rot[0] >> rot[1] >> rot[2];
				iss >> _bone_list[i]->_len;
				_bone_list[i]->_rest_pos = pos;
				_bone_list[i]->_rest_rot = buildRotationMatrix(rot*_angle_units);

#if 1
				cout << "bone " << i << ": " << _bone_list[i]->_name << endl;
				pos.print("   pos: ");
				rot.print("   rot: ");
				cout << "   len: " << _bone_list[i]->_len << endl;
#endif
				++i;
			}
		}


/*
		reset();

		ifstream fin( _Name.c_str() );
		if( !fin ) return;

		v3 pos, tgt, col;
		float scl;
		int parent;
		string str;
		AgentBone* curr_bone = 0;
		int brackets = 0;
		_total_channels = 0;
		string prev_bone_name("Root");

		do
		{
			// *** add bone ***
			if(!str.compare("ROOT") || !str.compare("JOINT") || !str.compare("End"))
			{
				curr_bone = addBone( curr_bone );
				string tempstr; fin >> tempstr;
				if(!str.compare("End"))
					str = prev_bone_name+str;
				else
					str = tempstr;
				prev_bone_name = str;
				curr_bone->setName(str);
				curr_bone->getID() = _bone_list.size()-1;
				_bone_map[str] = curr_bone;
			}

			if(!str.compare("{"))
			{
				brackets++;
			}

			if(!str.compare("}"))
			{
				brackets--;
				if(brackets>0) curr_bone = curr_bone->getParent();
			}

			if(!str.compare("OFFSET"))
			{
				fin >> str; float x = atof(str.c_str());
				fin >> str; float y = atof(str.c_str());
				fin >> str; float z = atof(str.c_str());
				curr_bone->getOffset() = v3(x,y,z);
			}

			if(!str.compare("CHANNELS"))
			{
				fin >> str; int channels = atoi(str.c_str());
				_total_channels += channels;
				for(int i=0; i<channels; ++i)
				{
					fin >> str;
					if(!str.compare("Xposition")) { curr_bone->setChannel(i,0); }
					if(!str.compare("Yposition")) { curr_bone->setChannel(i,1); }
					if(!str.compare("Zposition")) { curr_bone->setChannel(i,2); }
					if(!str.compare("Xrotation")) { curr_bone->setChannel(i,3); }
					if(!str.compare("Yrotation")) { curr_bone->setChannel(i,4); }
					if(!str.compare("Zrotation")) { curr_bone->setChannel(i,5); }
				}
			}

			fin >> str;

		} while( !fin.eof() && str.compare("RESTPOSE") );


		// load rest pose
		if( !str.compare("RESTPOSE") )
		{
			Animation* a = new Animation();
			_animations.push_back(a);

			a->getFrameDuration() = 1.f;

			Pose* p = new Pose();
			a->getPoses().push_back(p);
			for(int i=0; i<_total_channels; ++i)
			{
				fin >> str; float f = atof(str.c_str());
				p->_data.push_back(f);
			}
		}


		fin.close();

		_transforms.resize(_bone_list.size());

		cout << "Agent: number of bones = " << _bone_list.size() << endl;
*/
	}

	void loadAnimationHTR(const string _Name)
	{
		ifstream fin( _Name.c_str() );
		if( !fin ) return;

		string str;

		Animation* a = new Animation();
		_animations.push_back(a);

//		do { fin >> str; } while( str.compare("MOTION") );

		int num_frames;
		fin >> str; fin >> str; num_frames = atoi(str.c_str()); // a->getNumFrames() = atoi(str.c_str());
		fin >> str; fin >> str; fin >> str; a->getFrameDuration() = atof(str.c_str());

		for(int j=0; j<num_frames; ++j)
		{
			Pose* p = new Pose();
			a->getPoses().push_back(p);
			for(int i=0; i<_total_channels; ++i)
			{
				fin >> str; float f = atof(str.c_str());
				p->_data.push_back(f);
			}
		}

		fin.close();
	}

	void saveAnimationHTR(const int _Index,const string _Name)
	{
		ofstream fout( _Name.c_str() );
		if( !fout ) return;

		if(_Index>=_animations.size()) return;

		Animation* a = _animations[_Index];

		fout << "Frames: " << a->getPoses().size() << endl;
		fout << "Frame Time: " << a->getFrameDuration() << endl;

		for(int j=0; j<a->getPoses().size(); ++j)
		{
			Pose* p = a->getPoses()[j];

			for(int i=0; i<_total_channels; ++i)
			{
				fout << p->_data[i] << " ";
			}
			fout << endl;
		}

		fout.close();
	}

// ******************************************************************************************************

	void applyPose()
	{
		Animation::PoseList& p = _animations[_current_animation]->getPoses();
		int pose = ((int)floor(_pose_time))%p.size();
		int pose1 = (pose+1)%p.size();
		float lerp = _pose_time-float(pose);

		for(int ii=0,i=0; i<_bone_list.size(); ++i)
		{
			v3 off = _bone_list[i]->getOffset();

			m4 m = m4::identity;

			if( !_bone_list[i]->getParent() )
			{
				m = m4::rotation(_rotate)*m;
				m = m*m4::scaling(_scale);
				m = m*m4::translation(_translate);
			}

			m = m*m4::translation(off);

			v3 mov = v3(0,0,0);
			m4 rot = m4::identity;

			for(int j=0; j<6; ++j)
			{
				int c = _bone_list[i]->getChannel(j);
				if(c!=-1)
				{
					float f0 = p[pose]->_data[ii];
					float f1 = p[pose1]->_data[ii];

					if(f1-f0>180.f) f0 += 360.f;
					if(f0-f1>180.f) f1 += 360.f;
					ii++;
					float f = f0*(1.f-lerp) + f1*lerp;

					switch(c)
					{
						case 0: mov = mov+v3(f,0,0); break;
						case 1: mov = mov+v3(0,f,0); break;
						case 2: mov = mov+v3(0,0,f); break;
						case 3: rot = rot*m4::rotation(v3(1,0,0),deg2rad(f)); break;
						case 4: rot = rot*m4::rotation(v3(0,1,0),deg2rad(f)); break;
						case 5: rot = rot*m4::rotation(v3(0,0,1),deg2rad(f)); break;
					}
				}
			}

			m = m*m4::translation(mov)*rot;

			if( _float_length && i==0 )
			{
				m[12] *= 2.f;
				m[13] *= 2.f;
				m[14] *= 2.f;
			}

			_bone_list[i]->getLocalMat() = m;
		}
	}

	void update()
	{
		if(_animations.size()>_current_animation)
		{
			Animation* a = _animations[_current_animation];
			Animation::PoseList& p = a->getPoses();
			if( _playing ) _pose_time += (1.f/a->getFrameDuration())/60.f;
			if( _pose_time>p.size() ) _pose_time -= p.size();
			if( _bone_list.size()>0 && p.size()>0 ) applyPose();
		}

		for(unsigned int i=0; i<_bone_list.size(); ++i)
		{
			_bone_list[i]->calculateTransforms();

			if(_perform_skinning)
				_transforms[i] = _bone_list[i]->getRestToPoseMat().transpose();
			else
				_transforms[i] = m4::identity; // dont perform skinning
		}
	}

	void draw(const int _ViewedBone)
	{
		glLineWidth(2);
        glDisable(GL_DEPTH_TEST);
		glDisable( GL_LIGHTING );
		glDisable( GL_TEXTURE_2D );
		glBegin(GL_LINES);
		for( int i=0; i<_bone_list.size(); ++i )
		{
			AgentBone* current_bone = _bone_list[i];
			AgentBone* parent = current_bone->getParent();
			if(parent)
			{
				if(i==_ViewedBone) glColor3f(0,1,0); else glColor3f(1,0,0);

				v3 start = current_bone->getGlobalStartPos();
				v3 end = current_bone->getGlobalEndPos();

				glVertex3fv((float*)start.ptr());
				glVertex3fv((float*)end.ptr());
			}
		}
		glEnd();

		glEnable( GL_DEPTH_TEST );
		glEnable( GL_LIGHTING );
		glColor3f(1,1,1);
	}

	void dumpAgent()
	{
		for( int i=0; i<_bone_list.size(); ++i )
		{
			cout << "Bone " << i << ":" << endl;
			_bone_list[i]->dump();
			cout << endl;
		}
		cout << endl << endl;
	}

	void scale(const v3 _Scale)
	{
        for( int i=0; i<_bone_list.size(); ++i )
        {
			if( _bone_list[i]->getParent() )
			{
        		_bone_list[i]->getOffset() *= _Scale;
			}
		}
	}

	void translate(const v3 _Translate)
	{
        for( int i=0; i<_bone_list.size(); ++i )
        {
			if( !_bone_list[i]->getParent() )
			{
        		_bone_list[i]->getOffset() += _Translate;
			}
		}
	}

	void modifyRestPose(const int _Bone, const int _Channel, const float _Value)
	{
		if(_animations.size()>0)
		{
			Animation* a = _animations[_current_animation];
			Animation::PoseList& p = a->getPoses();
			int index = 0;
			for( int i=0; i<_bone_list.size()&&i!=_Bone; ++i )
			{
				for(int j=0; j<6; ++j)
				{
					int c = _bone_list[i]->getChannel(j);
					if(c!=-1)
					{
						index++;
					}
				}
			}

			for(int j=0; j<6; ++j)
			{
				int c = _bone_list[_Bone]->getChannel(j);
				if(c!=-1)
				{
					if(c==_Channel)
					{
						p[0]->_data[index] += _Value;
						break;
					}
					index++;
				}
			}
		}
	}

};


#endif
